home *** CD-ROM | disk | FTP | other *** search
/ HPAVC / HPAVC CD-ROM.iso / YAWPITCH.ZIP / YAWPITCH.CPP next >
C/C++ Source or Header  |  1995-11-27  |  4KB  |  130 lines
  1. /*  Roll pitch yaw demonstration */
  2. /*  by Karl Lager   */
  3. /*  11/27/95 */
  4.  
  5.  
  6. #include <math.h>
  7. #include <stdio.h>
  8.  
  9.  /* equatorial to alt-az:                                             */
  10.  /* altitude = arcsin(sin(dec)*sin(lat) + cos(dec)*cos(lat)*cos(ha))  */
  11.  /* azimuth = arccos( (sin(dec) - sin(lat)*sin(alt))/(cos(lat)*cos(alt)) )
  12.  
  13.  /* alt-az to equatorial                                              */
  14.  /* dec = arcsin(sin(alt)*sin(lat) + cos(alt)*cos(lat)*cos(az))       */
  15.  /* ha = arccos( (sin(alt) - sin(lat)*sin(dec))/(cos(lat)*cos(dec)) ) */
  16.  
  17.  
  18.  
  19. const double rad2deg = 180/M_PI;
  20. const double deg2rad = M_PI/180;
  21.  
  22. void alt_az_to_eq(double alt, double az,
  23.            double *dec, double *ha,
  24.            double lat);
  25.  
  26. void orient_ship(double *x, double *y, double *z,
  27.          double pitch, double yaw,double roll)
  28.          /* all angles are in radians */
  29. {double ha, ha2,dec, lat, lon,alt,az;
  30. // correct for reverse yaw angle(turn right = +)
  31. yaw = -yaw;                 // now all inputs are right hand angles
  32. /*// locate nose of ship*/
  33. lat = *x; lon = *y;
  34. /*// locate axis of rotation*/
  35. if (pitch == 0 && yaw == 0) {*z += roll; return;}
  36. alt = atan2(roll,sqrt(pitch*pitch+yaw*yaw));
  37. az = *z+ atan2(pitch,yaw);
  38. *z -= az;
  39. alt_az_to_eq(alt,az,&dec,&ha,lat);
  40. /*//locate az of ship*/
  41. lat = dec;
  42. alt_az_to_eq(*x,-ha,(&alt),&az,lat);
  43. az += sqrt(roll*roll+pitch*pitch+yaw*yaw);
  44. alt_az_to_eq(alt,az,&dec,&ha2,lat);
  45.  
  46. alt_az_to_eq(lat,ha2,(&alt),&az,dec);
  47. *x = dec; while (*x<0) *x += M_PI*2; while (*x>=M_PI*2) *x -= M_PI*2;
  48. *y = lon-ha-ha2; while(*y<0) *y += M_PI*2; while (*y>=M_PI*2) *y -= M_PI*2;
  49. *z -= az; while (*z<0) *z += M_PI*2; while (*z>=M_PI*2) *z -= M_PI*2;
  50. }
  51.  
  52.  
  53. void alt_az_to_eq(double alt, double az,
  54.            double *dec, double *ha,
  55.            double lat)
  56.  {
  57.  double sin_dec,cos_ha,sinalt,sinlat,coslat;
  58.  sinalt = sin(alt); sinlat = sin(lat); coslat = cos(lat);
  59.  sin_dec = sinalt*sinlat + cos(alt)*coslat*cos(az);
  60.  *dec = asin(sin_dec);
  61.  if (sin_dec == 1) cos_ha = 1 ; else
  62.  cos_ha = (sinalt - sinlat*sin_dec)/(coslat*cos(*dec)) ;
  63.  if (cos_ha > 1)
  64.    cos_ha = 1;
  65.  if (cos_ha < -1)
  66.    cos_ha = -1;
  67.  
  68.  *ha = acos(cos_ha);
  69.  if (sin(az) > 0) *ha = 2*M_PI - *ha;
  70.  }
  71.  
  72.  
  73. //_____________________________________________________________________
  74.  
  75.  
  76.  
  77. void main()
  78. {double roll,pitch,yaw,x,y,z;
  79.  int i;
  80.  
  81.  printf("enter pitch, yaw, roll=0 0 0 to exit.\n");
  82.  printf("enter xrot,yrot,zrot, pitch,yaw,roll :");
  83.  scanf("%lf%lf%lf%lf%lf%lf",&x,&y,&z,&pitch,&yaw,&roll);
  84.  while (roll || pitch || yaw)
  85.  {
  86.    x *= deg2rad;
  87.    y *= deg2rad;
  88.    z *= deg2rad;
  89.    roll *= deg2rad;
  90.    pitch *= deg2rad;
  91.    yaw *= deg2rad;
  92.  
  93.    orient_ship(&x,&y,&z,pitch,yaw,roll);
  94.  
  95.    x *= rad2deg;
  96.    y *= rad2deg;
  97.    z *= rad2deg;
  98.    printf(" xrot = %lf, yrot = %lf, zrot = %lf\n",x,y,z);
  99.    printf("enter pitch, yaw, roll (0, 0, 0 to exit.)\n");
  100.    scanf("%lf%lf%lf",&pitch,&yaw,&roll);
  101.  
  102.  }
  103. }
  104.  
  105. /*
  106.  note- The co-ordinate system I use here is:
  107.  with xrot = 0, yrot = 0, and zrot = 0
  108.  
  109.  x points right, y points up, and z points into the screen.
  110.  +pitch pulls up on the stick, +yaw turns right, and +roll roll to the right.
  111.  zrot, xrot, and yrot follow the directions of roll, pitch, and yaw.
  112.  In other words, zrot is right handed, xrot is right handed, and yrot
  113.  is lefthanded.
  114.  
  115.  This is a little bit oddball, since most texts tell you to pick one hand
  116.  and stick with it.  To change the handednes of a rotation variable,
  117.  the simplest way is to reverse the sign of the variable at the beginning
  118.  and end of the orientship procedure, the way I did with yaw.
  119.  
  120.  
  121.  By the way, if you want to move your object in the direction it is
  122.  pointing, you will need to convert the rotation angles to vectors.
  123.  
  124.  
  125.  x += speed*cos(xrot)*sin(yrot));
  126.  y += speed*sin(xrot));
  127.  z += speed*cos(xrot)*cos(yrot);
  128.  
  129.  
  130. */